/*
 * Copyright (c) 2025 lhjhit
 *
 * SPDX-License-Identifier: Apache-2.0
 */

/**
 * @file PWM-based servomotor control
 */

#include <zephyr/kernel.h>
// #include <zephyr/sys/printk.h>
#include <zephyr/logging/log.h>

#include <zephyr/drivers/pwm.h>

#include "servo_motor.h"

LOG_MODULE_REGISTER(MOTOR_MODULE, LOG_LEVEL_INF);

static const struct pwm_dt_spec servo_rotation = PWM_DT_SPEC_GET(DT_NODELABEL(servo0));
static const struct pwm_dt_spec servo_swing = PWM_DT_SPEC_GET(DT_NODELABEL(servo1));
static const uint32_t min_pulse_rotation = DT_PROP(DT_NODELABEL(servo0), min_pulse);
static const uint32_t max_pulse_rotation = DT_PROP(DT_NODELABEL(servo0), max_pulse);
static const uint32_t min_pulse_swing = DT_PROP(DT_NODELABEL(servo1), min_pulse);
static const uint32_t max_pulse_swing = DT_PROP(DT_NODELABEL(servo1), max_pulse);

bool is_working = false;
bool is_done = false;

// struct k_mutex motor_change_mutex;

static int pwm_initialization(void)
{
	// k_mutex_init(&motor_change_mutex);
	
	if (!pwm_is_ready_dt(&servo_rotation)) {
		LOG_ERR("Error: PWM device %s is not ready", servo_rotation.dev->name);
		return -1;
	}

	if (!pwm_is_ready_dt(&servo_swing)) {
		LOG_ERR("Error: PWM device %s is not ready", servo_swing.dev->name);
		return -1;
	}

	pwm_set_pulse_dt(&servo_rotation, min_pulse_rotation);
	pwm_set_pulse_dt(&servo_swing, min_pulse_swing);

	return 0;
}

void thread_movement(void *p1, void *p2, void *p3)
{
	// k_timeout_t sleep_time = K_MSEC(1000);
	k_timeout_t sleep_time = K_MSEC(500);

	uint32_t pulse_width = min_pulse_rotation;
	enum direction dir = UP;
	uint32_t pulse_width_swing = min_pulse_swing;
	enum direction dir_swing = UP;

	// int ret;
	
	ARG_UNUSED(p1);
	ARG_UNUSED(p2);
	ARG_UNUSED(p3);

	LOG_INF("Thread_movement: thread start...");

	if (pwm_initialization()) {
		LOG_ERR("Error: pwm_initialization failed!");
		return;
	}

	while (true)
	{
		// k_mutex_lock(&motor_change_mutex, K_FOREVER);
		
		if(is_working){

			is_done = false;

			if(pulse_width_swing <= min_pulse_swing) {
				dir_swing = UP;
				pulse_width_swing = min_pulse_swing;			
			}
			else if (pulse_width_swing >= max_pulse_swing) {
				dir_swing = DOWN;
				pulse_width_swing = max_pulse_swing;
			}

			while ((pulse_width_swing >= min_pulse_swing)&&(pulse_width_swing <= max_pulse_swing)){
				LOG_INF("Swing the model");
				pwm_set_pulse_dt(&servo_swing, pulse_width_swing);
				if (dir_swing == DOWN) pulse_width_swing -= STEP;
				else pulse_width_swing += STEP;
				
				k_sleep(sleep_time);
			}

			LOG_INF("Rotate the model");

			if (dir == DOWN) {
				if (pulse_width <= min_pulse_rotation) {
					dir = UP;
					// pulse_width = min_pulse_rotation;
					is_working = false;
					is_done = true;
				} else {
					pulse_width -= STEP;
				}
			} else {
				if (pulse_width >= max_pulse_rotation) {
					dir = DOWN;
					// pulse_width = max_pulse_rotation;
					is_working = false;
					is_done = true;
				} else {
					pulse_width += STEP;
				}
			}

			pwm_set_pulse_dt(&servo_rotation, pulse_width);
			k_sleep(sleep_time);
		}
		// k_mutex_unlock(&motor_change_mutex);
		k_sleep(sleep_time);
	}
}